/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0 

Bump_4WheeledVehicle.cpp
Bump_4WheeledVehicle management implementation.


*/


/* Class declaration */
#include "Bump_4WheeledVehicle.h"



Registrar Bump_4WheeledVehicle::registrar("BulletBaseVehicle", Bump_4WheeledVehicle::create);

// Required to simulate friction as car chassis not in actual contact with ground
const float Bump_4WheeledVehicle::DEFAULT_FRICTION_BRAKING_FORCE = 10.;


/**
 * Constructor for this derived class
 * Will be used if some attachment specific needs to be initialized comes up
 * Do NOT allocate memory here, let init() do that
 *
 * It is in fact currently used for a temporary vehicle simulation class which
 * should actually be derived from Bump_4WheeledVehicle.
 */
Bump_4WheeledVehicle::Bump_4WheeledVehicle()
{
	//Vehicle
	m_carChassis = NULL;
	m_vehicle = 0;
	m_wheelShape = 0;
	mass = 1500.f;
	transInitialChassis.setIdentity();


	//Engine parameters
	maxEngineForce = 3000.f;//this should be engine/velocity dependent
	maxBrakingForce = 500.f;
	gBrakingForce = DEFAULT_FRICTION_BRAKING_FORCE;
	gVehicleSteering = 0.f;
	gEngineForce = 0.f;  //initial force
	reverseEngineForce = -maxEngineForce/4;

	//Transmission : simple gear model which varies engine force only
	maxGear = 6; //Reverse and neutral indicated by 'R' & 'N'
	minGear = 1;
	m_gear = 1;

	//Steering parameters
	steeringIncrement = 0.04f;
	steeringClamp = 0.3f;

	//Wheel parameters
	wheelRadius = 1.5f;
	wheelHalfWidth = 1.182f;	 //actual width is 2.364(twice)
	wheelFriction = 1500;
	wheelFwdBackHalfDist = 2.7f;
    wheelLeftRightHalfDist = 1.3f;
	connectionHeight = 1.2f;
	rightIndex = 0;
	upIndex = 1;
	forwardIndex = 2;
	wheelDirectionCS0.setValue(0,-1,0);
	wheelAxleCS.setValue(-1,0,0);

	//Suspension parameters
	suspensionStiffness = 50.f;
	suspensionDamping = 20.0f;
	suspensionCompression = 2.4f;
	rollInfluence = 0.08f;//1.0f;
	suspensionRestLength = 0.9f;

	//Heights and initial location
	initialVehicleHeight = 2.0f; //Height from which vehicle is dropped on terrain at physics start
	chassisHeight = 1.2f; //Height of chassis above vehicle origin
	chassisHalfLength = 3.0f;
	chassisHalfWidth = 2.0f;
	chassisHalfHeight = 1.0f;

	//Raycast vehicle cant yet be deleted from physics world, though the VESSEL may be destroyed in
	//Orbiter, so deactivate it
	deleteIfInactive = false;



}


/**
 * Destructor, does nothing currently
 */
Bump_4WheeledVehicle::~Bump_4WheeledVehicle()
{
	//Destroy physics counterpart
	int rv = destroyBulletRigidBody();
	if(rv){
		sprintf(boutput, "Bump_4WheeledVehicle::exit: ERROR : Destroying rigid body failed for %s", strObjectName);
		oapiWriteLog(boutput);
	}

	//Vessel detach must be done manually if done midway as this can be called at sim end(detaching may hang Orbiter)

}


/**
 * Initialize a base attachment.
 * Creates a new BulletBase Attachment and inserts it in the Orbiter world & Bullet World
 *
 * The search or create functionality provided here has to be part of this derived class as
 * for the other derived class, there is no search required(Meshes must be created always)
 *
 * A attachment needs to be added to the BulletBase. There can be at most MAX_OBJECTS number of attachments
 * The check for whether the number of attachments is < MAX_OBJECTS is already done at
 * Bump_BulletBase::readCreatePhysicsFileObject()
 *
 * Also a single attachment point needs to be created in the vessel that will be attached.
 *
 * Both attachment points may need to be deleted midway, which brings up the question of how to delete
 * an attachment point in the middle of atBase[]. When attachment points are compacted, then all
 * attached objects have to be updated as they store the position in atBase[] for their VESSELs.
 *
 *
 *
 * TODO: Currently only searches and attaches, no create.
 * TODO: Check if vessel attachment point already exists, also remove attachment point whenever detached
 *
 */
int
Bump_4WheeledVehicle::clbkSetClassCaps(InitialObjectData& initialData)
{
	int rv;

	// This call MUST be made before anything else, sets base
	Bump_BaseObject::clbkSetClassCaps(initialData);

	oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: Initializing %s", strObjectName);


	//Create Orbiter object
	rv = attachOrbiterVESSEL();
	if(rv){
		oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Attaching Orbiter VESSEL failed %s", strObjectName);
		return 1;
	}


	//Read the configuration for this vehicle from orbiter vessel here ONLY
	//Orbiter vessel class will override all values, so it MUST have the right values
	//A map based system that tries to find the property may be a better idea.
	//bbv not available before attaching
	rv = readOrbiterVESSEL();
	if(rv){
		oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Reading options from Orbiter VESSEL failed %s", strObjectName);
		return 1;
	}

	//Choose the proper terrain generator as picked by the base.
	switch(bumpBase->terrainGenerator){
	case TT_PLANETCURVE:
		ground = new Bump_TerrainPatch(12, 12, 0.000002f, 0.000002f, 0.000008f, 0.000008f, 10.f, bumpBase);
		break;
	case TT_ORULEX:
		ground = new Bump_TerrainPatchOru(12, 12, 0.000002f, 0.000002f, 0.000008f, 0.000008f, 10.f, bumpBase);
		break;
	case TT_D3D11:
		break;
	default:
		//No terrain!!, physics controlled objects will fall through
		break;
	}

	if(!ground){
		oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Could not add ground for %s", strObjectName);
		return 2;
	}

	//Create terrain patch from initial equatorial position of vessel
	rv = ground->createPatch(lng, lat);
	if(rv){
		//detach VESSEL that would already have been attached at this point
		if(detachOrbiterVESSEL()){
			//OK, now something is *REALLY* wrong, just inform & return...
			oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Detaching Orbiter VESSEL failed after failure to " \
					"create physics counterpart %s", strObjectName);
		}

		oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Could not add ground for %s", strObjectName);
		return 2;
	}



	//Create physics counterpart, if this fails, should detach Orbiter VESSEL to return it to initial state
	rv = createBulletRigidBody();
	if(rv){
		//detach VESSEL that would already have been attached at this point
		if(detachOrbiterVESSEL()){
			//OK, now something is *REALLY* wrong, just inform & return...
			oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Detaching Orbiter VESSEL failed after failure to " \
					"create physics counterpart %s", strObjectName);
		}

		oapiWriteLogV("Bump_4WheeledVehicle::clbkSetClassCaps: ERROR : Creating rigid body failed %s", strObjectName);
		return 3;
	}


	//Wheel rendering
	m_ShapeDrawer = phyEngine->getShapeDrawer();
	phyEngine->addExternalRenderObject(this, renderFuncPos);


	//get vessel structure and set controlled VESSEL's state to landed
	VESSELSTATUS2 vs;
	memset(&vs, 0, sizeof(VESSELSTATUS2));
	vs.version = 2;

	vAttached->GetStatusEx(&vs);
	vs.status = 1;
	vAttached->DefSetStateEx(&vs);



	//Only after Orbiter AND Bullet successful, set physics to true
	physicsInited = true;


	return 0;
}


/**
 *
 */
int
Bump_4WheeledVehicle::readOrbiterVESSEL(void)
{

	//Engine parameters
	maxEngineForce = bbv->maxEngineForce;//this should be engine/velocity dependent
	maxBrakingForce = bbv->maxBrakingForce;
	reverseEngineForce = bbv->reverseEngineForce;
	mass = bbv->mass;

	//Transmission : simple gear model which varies engine force only
	maxGear = bbv->maxGear; //Reverse and neutral indicated by 'R' & 'N'
	minGear = bbv->minGear;

	//Steering parameters
	steeringIncrement = bbv->steeringIncrement;
	steeringClamp = bbv->steeringClamp;


	//Physics world location
	if(bbv->physicsPositionAvailable){
		transInitialChassis.setIdentity();
		transInitialChassis.setOrigin(btVector3((btScalar)bbv->pos.x,
				                     (btScalar)bbv->pos.y + initialVehicleHeight,
				                     (btScalar)bbv->pos.z));

		//Do not use quaternions while restoring rotations, doesnt work!
		transInitialChassis.getBasis().setEulerYPR(bbv->yaw, bbv->pitch, bbv->roll );
	}
	else
		transInitialChassis.setOrigin(
				btVector3((btScalar)rposVesselStartingPos.x, 
				          (btScalar)rposVesselStartingPos.y, 
						  (btScalar)rposVesselStartingPos.z) +
				bumpBase->simBoxOffset +
				btVector3(0., initialVehicleHeight, 0.));


	return 0;

}


/**
 * Updates a base attachment
 * Primarily move the vessel mesh into position according to latest transform by setting
 * attachment point parameter.
 * Needs to read and process any commands too from the containing base
 */
int
Bump_4WheeledVehicle::clbkPreStep(double SimT, double SimDT, double MJD)
{

	btRigidBody *rb;
	VECTOR3 global;
	btScalar m[16];


//	oapiWriteLogV("Bump_4WheeledVehicle::poststep: The next sim step will advance the sim by %f secs", SimDT);


	if(physicsInited){

		//Main world transform of the vehicle
		rb = m_vehicle->getRigidBody();
		btTransform transVehicle = rb->getCenterOfMassTransform();
		transVehicle.getOpenGLMatrix(m);

		// Convert transform to needed positions in Cartesian and polar co-ords
		btVector3 trueChassisPos = btVector3(m[12], m[13],	m[14]) - bumpBase->simBoxOffset;
		bumpBase->bb->Local2Global(_V(m[12], m[13], m[14]), gposVessel);
		oapiGlobalToEqu(bumpBase->hObjRefBody, gposVessel, &lng, &lat, &rad);


		bumpBase->bb->SetAttachmentParams(atBase,
						 _V(trueChassisPos.x(),
							trueChassisPos.y(),
							trueChassisPos.z()),
						 _V(m[4], m[5], m[6]),							//y axis
						 _V(m[8], m[9], m[10]));						//z axis

		// Push the position to the orbiter VESSEL3 object
		bbv->pos = _V(trueChassisPos.x(),
					  trueChassisPos.y(),
					  trueChassisPos.z());

		rb->getCenterOfMassTransform().getBasis().getEulerYPR(bbv->yaw, bbv->pitch, bbv->roll);

		//Now for the wheels
		for (int i=0;i<m_vehicle->getNumWheels();i++){

			//synchronize the wheels with the (interpolated) chassis worldtransform
			m_vehicle->updateWheelTransform(i, true);

			btTransform transWheel = m_vehicle->getWheelInfo(i).m_worldTransform;
			transWheel.getOpenGLMatrix(m);

			btVector3 trueWheelPos = btVector3(m[12], m[13],	m[14]) - bumpBase->simBoxOffset;
			bumpBase->bb->Local2Global( _V(trueWheelPos.x(),
										   trueWheelPos.y(),
										   trueWheelPos.z()),
										   global);
			vAttached->Global2Local(global, bbv->local[i]);

			//oapiWriteLogV("local[%d] : %f, %f, %f", i, bbv->local[i].x, bbv->local[i].y, bbv->local[i].z);

			bbv->wheelRot[i] = m_vehicle->getWheelInfo(i).m_rotation;

			// Place collision meshes in Orbiter window
			btTransform transShape = transWheel;
			transShape.setOrigin(trueWheelPos);
			collMshWheel[i].drawSimpleShape(transShape);

		}

		//Insert Engine info, move to vehicle class
		bbv->gear = m_gear;
		bbv->speed = m_vehicle->getCurrentSpeedKmHour();
		bbv->steering = m_vehicle->getSteeringValue(0);

		bbv->updateVessel = true;


		//Physics stuff, should be in prestep()---------------------------------

		//Process command stored in orbiter vessel
		processCommand();


		//Update vehicle
		int wheelIndex = 2;
		m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
		m_vehicle->setBrake(gBrakingForce,wheelIndex);
		wheelIndex = 3;
		m_vehicle->applyEngineForce(gEngineForce,wheelIndex);
		m_vehicle->setBrake(gBrakingForce,wheelIndex);

		wheelIndex = 0;
		m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);
		wheelIndex = 1;
		m_vehicle->setSteeringValue(gVehicleSteering,wheelIndex);



		//Update terrain
		if(ground)
			int t = ground->updatePatch(lng, lat, rad);


		//Show collision mesh
		btTransform transFinal = transVehicle;
		transFinal.setOrigin(trueChassisPos + transLocalChassis.getOrigin());
		//collMshChassis.drawCompoundShape(transFinal);

		// If we reach till here then we can chk camera focus
		if(oapiGetFocusObject() == hObjThis)
			clbkSetFocus();


	}//physics inited

	return 0;
}


/**
 *	Process keyboard input any other messages
 */
int
Bump_4WheeledVehicle::processCommand()
{
	if(bbv->validKeyInput){

		if(bbv->keyInput.down){

			int rv;

			//Command
			switch (bbv->keyInput.key)
			{
				case OAPI_KEY_UP:
					m_carChassis->setActivationState(DISABLE_DEACTIVATION);
					gBrakingForce = DEFAULT_FRICTION_BRAKING_FORCE;
					switch(m_gear){
						case 'N': gEngineForce = 0;					break;
						case 'R': gEngineForce = reverseEngineForce;	break;
						default:  gEngineForce = maxEngineForce/(maxGear/m_gear);	break;
					}
					break;

				case OAPI_KEY_DOWN:
					gBrakingForce = maxBrakingForce;
					gEngineForce = 0.f;
					break;

				case OAPI_KEY_RIGHT:
					gVehicleSteering += steeringIncrement;
					if (	gVehicleSteering > steeringClamp)
							gVehicleSteering = steeringClamp;
					//bbv->keyInput.key = NO_KEY;				//A possible syncing and message passing mechanism
					break;

				case OAPI_KEY_LEFT:
					gVehicleSteering -= steeringIncrement;
					if (	gVehicleSteering < -steeringClamp)
							gVehicleSteering = -steeringClamp;
					//bbv->keyInput.key = NO_KEY;
					break;

			
				case OAPI_KEY_Z:
					m_carChassis->setActivationState(DISABLE_DEACTIVATION);
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_SPACE:
					gVehicleSteering = 0;
					break;

				case OAPI_KEY_W:
					if(m_gear == 'N' || m_gear == 'R')
						m_gear = 1;
					else
						m_gear = (m_gear+1) > maxGear ? m_gear : m_gear+1;
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_S:
					if(m_gear == 'N' || m_gear == 'R')
						m_gear = 1;
					else
						m_gear = (m_gear-1) < minGear ? m_gear : m_gear-1;
					bbv->keyInput.key = NO_KEY;
					break;

				case OAPI_KEY_A:
					m_gear = 'N';
					break;

				case OAPI_KEY_D:
					m_gear = 'R';
					break;

				case OAPI_KEY_G:
					//chks in control-map for vessel wrappers & removes, this causes del of attach pts in dtor of wrapper
					rv = bumpBase->dropBaseObject(bbv->vShuttle->GetHandle());
					if(rv){
						sprintf(boutput, "Bump_4WheeledVehicle::processCommand: ERROR : Failed to DROP %p", bbv->vShuttle->GetHandle());
						oapiWriteLog(boutput);
					}
					break;

				
				case NO_KEY:
					gEngineForce = 0.f;
					gBrakingForce = DEFAULT_FRICTION_BRAKING_FORCE;
					break;

				default:
					break;
			}
		}
		else{
			switch (bbv->keyInput.key)	//this key was released
			{
				case OAPI_KEY_W:			//gear changes
				case OAPI_KEY_S:
				case OAPI_KEY_A:
				case OAPI_KEY_D:
					gEngineForce = 0.f;
					gBrakingForce = DEFAULT_FRICTION_BRAKING_FORCE;
					break;

				case OAPI_KEY_UP: 			//accelerator released
					gEngineForce = 0.f;
					break;

				case OAPI_KEY_DOWN:			//brake released
					gBrakingForce = DEFAULT_FRICTION_BRAKING_FORCE;
					break;



				default:
					break;
			}


			bbv->keyInput.key = NO_KEY;
		}
	}

	bbv->validKeyInput = false;

	return 0;
}


/**
 * Attach the VESSEL to the BulletBase in the Orbiter world
 *
 */
int
Bump_4WheeledVehicle::attachOrbiterVESSEL()
{
	unsigned int i;
	unsigned int numSimVessels = oapiGetVesselCount();
	OBJHANDLE hObj;
	char strVesselName[MAX_STRLEN], strAttID[8];
	int bbvCount = 0;
	VECTOR3 gposVesselPos;


	//Search the Orbiter sim for this vessel
	for (i = 0; i<numSimVessels; i++) {

		//Assuming hObj will be non-NULL
		hObj = oapiGetVesselByIndex(i);
		oapiGetObjectName(hObj, strVesselName, MAX_STRLEN);

		oapiWriteLogV("Bump_4WheeledVehicle::attachOrbiterVESSEL: %3d. Found : %s", i+1, strVesselName);
		

		//Does the vessel name match this object's name ?
		if(strcmp(strVesselName, strObjectName) == 0){

			//VESSEL not yet attached but soon will be, hopefully!
			vAttached = (VESSEL3*)oapiGetVesselInterface(hObj);

			//Store vessel's equ position for terrain patch creation
			vAttached->GetEquPos(lng, lat, rad);

			//oapiWriteLogV("Bump_4WheeledVehicle::attachOrbiterVESSEL: %s is at : %f, %f, %f", strVesselName, lng, lat, rad);

			//Convert Vessel's global position into BulletBase relative position
			//Only the relative vessel position is actually stored, its not updated after this
			oapiEquToGlobal(bumpBase->hObjRefBody, lng, lat, rad, &gposVesselPos);
			bumpBase->bb->Global2Local(gposVesselPos , rposVesselStartingPos);

			/*oapiWriteLogV("Bump_4WheeledVehicle::attachOrbiterVESSEL: rposVesselStartingPos is : %f, %f, %f, rad = %f",
					V3ARGS(rposVesselStartingPos), radius);*/

		/*	rposVesselStartingPos.x = 5;
			rposVesselStartingPos.y = 0;
			rposVesselStartingPos.z = 0;*/


			//Create an attachment point at the base, sep parent/child lists exist, take next avail. pos.
			sprintf(strAttID, "BB%d", bumpBase->bb->AttachmentCount(FALSE));
			atBase = bumpBase->bb->CreateAttachment(false,
					rposVesselStartingPos,
					_V(0, -1, 0),
					_V(0, 0, 1),
					 strAttID);



			//Create an attachment point for the VESSEL	, -1 temp. for debug.
			atVessel = vAttached->CreateAttachment(true, _V(0, 0, 0),
															 _V(0, -1, 0),
															 _V(0, 0, 1),
															 "BBA_CHILD");

			//Attachment points are valid ?
			if(atBase && atVessel){

				//Try attaching, child hObj is first parameter
				if(bumpBase->bb->AttachChild(hObj, atBase, atVessel)){

					//Success!, attached
					sprintf(boutput, "Bump_4WheeledVehicle::attachOrbiterVESSEL: Added %s to %s at attachment handle position %d",
										strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
					oapiWriteLog(boutput);

					bbv = (BulletBaseVehicle*)vAttached;

					//Return non-zero only if object was found and attached, anything else must return 1
					return 0;
				}
				else{ //Failure, could not attach
					sprintf(boutput,"Bump_4WheeledVehicle::attachOrbiterVESSEL: ERROR : Attach failed: p:%d c:%d &p:%p, &c:%p",
							bumpBase->bb->AttachmentCount(FALSE),
							vAttached->AttachmentCount(TRUE),
							atBase,
							atVessel);
					oapiWriteLog(boutput);

					//Should NULL all imp ptrs, to prevent use
					physicsInited = false;
					bbv = NULL;
					vAttached = NULL;
					atBase = NULL;
					atVessel = NULL;
				}



			}
			else{
				//One of the attachment handles could not be created
				sprintf(boutput,"Bump_4WheeledVehicle::attachOrbiterVESSEL: ERROR : Attachment handle creation failed: p:%d c:%d &p:%p, &c:%p",
											bumpBase->bb->AttachmentCount(FALSE),
											vAttached->AttachmentCount(TRUE),
											atBase,
											atVessel);
				oapiWriteLog(boutput);
			}

		}//end-if(strcmp...

	}//end-for...


	//If we reach here, the vessel was not found or there was an error during attachment, return error
	return 1;
}


/**
 * Detach the VESSEL from the BulletBase in the Orbiter world
 *
 */
int
Bump_4WheeledVehicle::detachOrbiterVESSEL()
{

	//Attachment points are valid ?
	if(atBase && atVessel){

		//Try detaching child
		if(bumpBase->bb->DetachChild(atBase)){
			//Success!, detached
			sprintf(boutput, "Bump_4WheeledVehicle::detachOrbiterVESSEL: Detached VESSEL %s from %s, "\
					"attachment count in base now %d",
					strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
			oapiWriteLog(boutput);

			//Delete the attachment handles
			if(!(bumpBase->bb->DelAttachment(atBase)) || !(vAttached->DelAttachment(atVessel))){
				sprintf(boutput,"Bump_4WheeledVehicle::detachOrbiterVESSEL: ERROR : Deletion failed for 1 attachment point: p:%d c:%d &p:%p, &c:%p",
									bumpBase->bb->AttachmentCount(FALSE),
									vAttached->AttachmentCount(TRUE),
									atBase,
									atVessel);
				oapiWriteLog(boutput);
			}


			//These must be nulled after successful detach ONLY
			physicsInited = false;
			bbv = NULL;
			vAttached = NULL;
			atBase = NULL;
			atVessel = NULL;


		}
		else{ //Failure, could not detach
			sprintf(boutput,"Bump_4WheeledVehicle::detachOrbiterVESSEL: ERROR : Detach failed: p:%d c:%d &p:%p, &c:%p",
					bumpBase->bb->AttachmentCount(FALSE),
					vAttached->AttachmentCount(TRUE),
					atBase,
					atVessel);
			oapiWriteLog(boutput);

			return 1;
		}

		oapiWriteLog(boutput);

	}
	else{
		//One of the attachment handles is not valid, cannot detach
		sprintf(boutput,"Bump_4WheeledVehicle::detachOrbiterVESSEL: ERROR : Detachment failed due to invalid attachment handles: p:%d c:%d &p:%p, &c:%p",
									bumpBase->bb->AttachmentCount(FALSE),
									vAttached->AttachmentCount(TRUE),
									atBase,
									atVessel);
		return 1;
	}

	return 0;
}


/**
 * Create the physics counterpart in the Bullet world
 *
 */
int
Bump_4WheeledVehicle::createBulletRigidBody()
{
	bool isFrontWheel;
	char meshName[MAX_STRLEN];

	//The compound shape for the chassis
	compoundChassisShape = new btCompoundShape();
	//bumpBase->m_collisionShapes->push_back(compoundChassisShape);

	//The chassis shape to be added to the compound shape
	chassisBoxShape = new btBoxShape(btVector3(chassisHalfWidth,
											chassisHalfHeight,
											chassisHalfLength));
	//bumpBase->m_collisionShapes->push_back(chassisBoxShape);

	//localTrans for the chassis shape
	//It effectively shifts the center of mass with respect to the chassis
	transLocalChassis.setIdentity();
	transLocalChassis.setOrigin(btVector3(0, chassisHeight,0));

	//Add chassis shape to compound shape
	compoundChassisShape->addChildShape(transLocalChassis, chassisBoxShape);

	m_carChassis = phyEngine->localCreateRigidBody(mass, transInitialChassis, compoundChassisShape);

	//Now the wheels, first the cylinder shape for the wheels
	m_wheelShape = new btCylinderShapeX(btVector3(wheelHalfWidth, wheelRadius, wheelRadius));

	//Create vehicle
	m_vehicleRayCaster = new btDefaultVehicleRaycaster(bumpBase->m_dynamicsWorld);
	m_vehicle = new btRaycastVehicle(m_tuning,m_carChassis,m_vehicleRayCaster);

	//Never deactivate the vehicle
	m_carChassis->setActivationState(DISABLE_DEACTIVATION);

	bumpBase->m_dynamicsWorld->addVehicle(m_vehicle);

	//choose coordinate system
	m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);

	//forward left wheel
	isFrontWheel=true;
	btVector3 connectionPointCS0(wheelLeftRightHalfDist, connectionHeight, wheelFwdBackHalfDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//forward right wheel
	connectionPointCS0 = btVector3(-wheelLeftRightHalfDist, connectionHeight, wheelFwdBackHalfDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	isFrontWheel = false;
	//backward left wheel
	connectionPointCS0 = btVector3(wheelLeftRightHalfDist, connectionHeight, -wheelFwdBackHalfDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//backward right wheel
	connectionPointCS0 = btVector3(-wheelLeftRightHalfDist, connectionHeight, -wheelFwdBackHalfDist);
	m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,m_tuning,isFrontWheel);

	//Set wheel properties for all 4 wheels
	for (int i=0;i<m_vehicle->getNumWheels();i++){
		btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = suspensionStiffness;
		wheel.m_wheelsDampingRelaxation = suspensionDamping;
		wheel.m_wheelsDampingCompression = suspensionCompression;
		wheel.m_frictionSlip = wheelFriction;
		wheel.m_rollInfluence = rollInfluence;
		
		sprintf(meshName, "%s_Wheel%d", strObjectName, i);
		collMshWheel[i].addSimpleShape(bumpBase, m_wheelShape, meshName);
	}


	//Celestial body specific gravity
	m_carChassis->setGravity(btVector3((btScalar)bumpBase->G.x,
									   (btScalar)bumpBase->G.y,
									   (btScalar)bumpBase->G.z));


	//m_carChassis->setGravity(btVector3(0., -10, 0.));

	//Set the collision mesh
	sprintf(meshName, "%s_Chassis", strObjectName);
	collMshChassis.addCompoundShape(bumpBase, chassisBoxShape, meshName, NULL);



	return 0;
}


/*
 * Wheels rendering for the vehicle
 *
 * DO NOT MAKE ANY ORBITER CALLS FROM HERE. THIS IS A DIFFERENT THREAD
 */
int
Bump_4WheeledVehicle::clbkPhysicsRender(void)
{
	int i;
	btScalar m[16];
	btVector3 wheelColor(1,0,0);
	btVector3	worldBoundsMin,worldBoundsMax;
	bumpBase->m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);



	for (i=0;i<m_vehicle->getNumWheels();i++)
	{
		//synchronize the wheels with the (interpolated) chassis worldtransform
		m_vehicle->updateWheelTransform(i,true);
		//draw wheels (cylinders)
		m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
		m_ShapeDrawer->drawOpenGL(m, m_wheelShape,wheelColor, phyEngine->getDebugMode(),worldBoundsMin,worldBoundsMax);
	}

	return 0;
}

/**
 * De-allocates memory being consumed by the raycast vehicle so it can be
 * created again by createBulletRigidBody()
 *
 * This is a very important function. If memory is not freed up, Orbiter sim will not
 * exit properly and the Launchpad will not appear leading to user frustration!
 *
 * Here we de-allocate only the sub class specific stuff which have not been freed by
 * phyEngine.clientResetScene(), which is called before Bump_BulletBase::exit();
 *
 * Thus rigid bodies(derived from CollisionObjects), motion states are already removed
 * by the time we come here, DO NOT remove them again !
 *
 * The stuff which should be removed are any sub class specific classes used to model
 * this object like m_vehicleRayCaster & m_vehicle. Also any collision shapes not pushed
 * into m_collisionShapes[] during init(), needs to be freed here, such as the wheelShape
 */
int
Bump_4WheeledVehicle::destroyBulletRigidBody()
{
	physicsInited = false;

	//Remove Physics stuff (reverse order of creation),
	//
/*	bumpBase->m_dynamicsWorld->removeAction(m_vehicle);

	//The main rigid body : there may be a BUG here, this may have to be removed
	//from the dynamics world
	if (m_carChassis && m_carChassis->getMotionState()){
		delete m_carChassis->getMotionState();

		sprintf(boutput,"Bump_4WheeledVehicle::destroyBulletRigidBody: Motion state deleted for %s",
							strObjectName);
		oapiWriteLog(boutput);
	}

	//m_carChassis->getColli
	//m_dynamicsWorld->removeCollisionObject(
	bumpBase->m_dynamicsWorld->removeCollisionObject(m_carChassis);

	delete m_carChassis;
*/

	delete ground;

	//Vehicle specific objects
	delete m_vehicleRayCaster;
	delete m_vehicle;

	//These shapes were not added to m_collisionShapes[], so they have to be removed here
	//They were not added so that they could be specifically delete during reset()
	delete chassisBoxShape;
	delete compoundChassisShape;
	delete m_wheelShape;

	return 0;
}


/**
 * Transform debugging function, can rip through the dynamics world and get transforms in the worst case
 * Used for investigating issues arising from incorrect extraction of transforms from the Bullet structs
 */
void
Bump_4WheeledVehicle::getTransforms(void)
{

	btScalar m[16];
	const int numObjects = bumpBase->m_dynamicsWorld->getNumCollisionObjects();

	//Go through all collision objects
	for(int i=0;i<numObjects;i++){
		//Common properties
		btCollisionObject*	colObj = bumpBase->m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody*		body   = btRigidBody::upcast(colObj);
		const btCollisionShape* shape = colObj->getCollisionShape();

		if(body&&body->getMotionState()){
			btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();

			if(shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE){			//chassis
				myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);

				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
				btTransform childTrans = compoundShape->getChildTransform(0);
				//childTrans.getOpenGLMatrix(pkt.childMat);
				shape = compoundShape->getChildShape(0);
				if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){
					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
					//pkt.halfExtent = boxShape->getHalfExtentsWithoutMargin(); //with and without margin makes a difference in size

				}
			}
			else if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){		//collision box
				//const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
				//myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(pkt.m_box);
				//pkt.box_halfExtent = boxShape->getHalfExtentsWithoutMargin();
			}

		}
		else{
			if(shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE){			//chassis
				colObj->getWorldTransform().getOpenGLMatrix(m);

				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
				btTransform childTrans = compoundShape->getChildTransform(0);
				//childTrans.getOpenGLMatrix(pkt.childMat);
				shape = compoundShape->getChildShape(0);
				if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){
					const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
					//pkt.halfExtent = boxShape->getHalfExtentsWithMargin();

				}
			}
			else if(shape->getShapeType() == BOX_SHAPE_PROXYTYPE){		//collision box
				//const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
				//colObj->getWorldTransform().getOpenGLMatrix(pkt.m_box);
				//pkt.box_halfExtent = boxShape->getHalfExtentsWithMargin();
			}
		}
	}
}



